
lambda = 1;
%N = 8;
N
d = lambda / 2;

mu = 0.05 * pi / 180;
ML = 1000;

h = modem.pskmod('M', 4);              % Modulator object
msg = randi([0 3],ML,1);               % Modulating message
x = (modulate(h,msg) * ones(1,N)).';
x = x ./ N;

% FROM HERE, IMPLEMENTATION FROM KOEN
M  = zeros(N,N);                    % Generate matrix of form:
                                    %   0           ...   n_0-n_{N-1};   
for r=1:(N)                         %   .           0                ; 
   M(r,:)=(r-1:(-1):-N+r);          %  n_{N-1}-n_0  ...             0;]
end


nv           = (0:1:N-1).';


phi_const = (1j*2*pi*d)/lambda;

% Define phi and phi_prime (as in paper)
%phi         = @(theta) phi_const*sin(theta);
%phi_p       = @(theta) phi_const*cos(theta);

R2 = 1;
%a_prime         = @(theta_k,xv) real( (xv'*( phi_p(theta_k).*M.*exp(M.*phi(theta_k)) )*xv));
%theta_k_next    = @(theta_k,xv) theta_k - mu.*(abs( exp(phi(theta_k).*nv)'*xv )*abs(exp(phi(theta_k).*nv)'*xv ) - R2) .* a_prime(theta_k,xv);
% END HERE

g_phi_const = gpuArray(phi_const);
g_M_nv = gpuArray(horzcat(M,M,nv,nv));
g_x = gpuArray(x); 


k=1;
theta = -14 * pi / 180;
values = zeros(ML - 1, 1);
values = gpuArray(values);
 while k < ML,
    if mod(k,100) == 0
        k
    end

    values(k) = theta;
    
    % theta_k - mu.*(abs( exp(phi(theta_k).*nv)'*xv )*abs(exp(phi(theta_k).*nv)'*xv ) - R2) .* real( (xv'*( phi_p(theta_k).*M.*exp(M.*phi(theta_k)) )*xv));
    g_theta_sin = gpuArray(  repmat(theta,N,N+2) );
    g_theta_cos = gpuArray(  repmat(theta,N,N) );
    % sin(theta) (3x)
    g_sin_theta = sin( g_theta_sin );
    % cos(theta) (1x)
    g_cos_theta = cos( g_theta_cos );
    % phi_const*result (3x)


    %g_phi = arrayfun( @const_mult, horzcat(g_cos_theta,g_sin_theta),g_phi_const);
    g_phi = horzcat(g_cos_theta,g_sin_theta).*g_phi_const;
    
    % theta_k - mu.*(abs( exp(result.*nv)'*xv )*abs(exp(result.*nv)'*xv ) - R2) .* real( (xv'*( result.*M.*exp(M.*result) )*xv));
    % result.*nv (2x) result.*M (2x) (scalar)
    g_M_nv_g_phi = g_M_nv.*g_phi;

    % theta_k - mu.*(abs( exp(result)'*xv )*abs(exp(result)'*xv ) - R2) .* real( (xv'*( result.*exp(result) )*xv));
    % exp(result) (3x) (exp op matrix)
    g_exp_result = exp(g_M_nv_g_phi(:,N+1:2*N+2));

    % theta_k - mu.*(abs( result1'*xv )*abs(result1'*xv ) - R2) .* real( (xv'*( result1.*result )*xv));
    % result1.*result (scalar * vector -> vector) 1x
    g_B_prime = g_M_nv_g_phi(:,1:N).*g_exp_result(:,1:N);

    % theta_k - mu.*(abs( result1'*xv )*abs(result1'*xv ) - R2) .* real( (xv'*( result2 )*xv));
    % xv'*result2  (vector*matrix -> vector) 1x
    g_xv = g_x(:, k);
    g_xv_prime = g_xv';
    g_xv_prime_g_B_prime = g_xv_prime*g_B_prime;

    % theta_k - mu.*(abs( result1'*xv )*abs(result1'*xv ) - R2) .* real( (result3*xv));
    % result1'*xv, result3*xv (3x) (vector*vector->scalar)
    g_yk = g_exp_result(:,N+1)'*g_xv;
    g_to_real = g_xv_prime_g_B_prime * g_xv;

    % theta_k - mu.*(abs( result4 )*abs(result4 ) - R2) .* real( result4);
    % abs(result4) (2x)
    g_abs_value = abs(g_yk);

    % theta_k - mu.*(result5*result5 - R2) .* real(result4);
    % real(result4)
    g_real = real(g_to_real);

    % theta_k - mu.*(result5*result5 - R2) .* result6;
    % result5*result5, mu*result6 (2x) (scalar*scalar->scalar)
    g_normal_mult = horzcat(g_abs_value, mu) .* horzcat(g_abs_value, g_real);

    % theta_k - result8.*(result7 - R2);
    % result7 - R2
    % theta_k - result8.*(result9);
    % result8.*(result9)
    % theta_k - result10;
    theta = theta - g_normal_mult(2).*(g_normal_mult(1) - R2);

    
    
    k=k+1;
    
 end

plot(values);